/*
 * _301.c
 *
 * Created: 5/9/2015 7:12:58 PM
 *  Author: Jeevake Attapattu
 */ 

#define F_CPU 8000000UL
#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>
#include <util/twi.h>
#include "i2cmaster.h"
#include <math.h>
#include <string.h>


#include <pololu/orangutan.h>


#define MPU60501  0xD0     // (0x68 << 1) I2C slave address
#define MPU60502  0xD2     // (0x69 << 1) I2C slave address

unsigned char ret;            // return value

char outs[50];

//********************************//



void usart_init (void)
{
	//synchronous usart, transmit 8-bit data
	UCSR0C = ((1<<UCSZ01)|(1<<UCSZ00)|(1<<UMSEL00));
	//9600 Baud Rate from 8MHz clock
	UBRR0H = 0x01;
	UBRR0L = 0xA0;
	UCSR0B = (1<<TXEN0);	//enable transmitter
}
void USART_tx_string (char *data)
{
	while((*data!='\0')){
		while(!(UCSR0A&(1<<UDRE0)));			//wait until transmit register is empty
		UDR0 = *data;
		data++;
	}
}


void MPU6050_writereg(uint8_t accel, uint8_t reg, uint8_t val)
{
	i2c_start(accel+I2C_WRITE);
	i2c_write(reg);  // go to register e.g. 106 user control
	i2c_write(val);  // set value e.g. to 0100 0000 FIFO enable
	i2c_stop();        // set stop condition = release bus
}


uint16_t MPU6050_readreg(uint8_t accel, uint8_t reg)//read unsigned 16 bits
{
	i2c_start_wait(accel+I2C_WRITE); // set device address and write mode
	i2c_write(reg);                                  // ACCEL_OUT
	i2c_rep_start(accel+I2C_READ);    // set device address and read mode
	int raw = i2c_readAck();                    // read one intermediate byte
	raw = (raw<<8) | i2c_readNak();        // read last byte
	i2c_stop();
	return raw;
}

int16_t MPU6050_signed_readreg(uint8_t accel, uint8_t reg)//read signed 16 bits
{
	i2c_start_wait(accel+I2C_WRITE); // set device address and write mode
	i2c_write(reg);                                  // ACCEL_OUT
	i2c_rep_start(accel+I2C_READ);    // set device address and read mode
	char raw1 = i2c_readAck();                    // read one intermediate byte
	int16_t raw2 = (raw1<<8) | i2c_readNak();        // read last byte
	i2c_stop();
	return raw2;
}



void Init_MPU6050(uint8_t accel)
{

	ret = i2c_start(accel+I2C_WRITE);       // set device address and write mode
	if ( ret )
	{

		snprintf(outs,sizeof(outs),"failed to issue start condition \n\r");
		USART_tx_string(outs);
		i2c_stop();
	}
	else
	{
		/* issuing start condition ok, device accessible */
		MPU6050_writereg(accel, 0x6B, 0x00); // reg 107 set value to 0000 0000 and wake up sensor
		MPU6050_writereg(accel, 0x19, 0x07); // reg 25 sample rate divider set value to 0000 1000 for 1000 Hz
		MPU6050_writereg(accel, 0x1C, 0x18); // reg 28 acceleration configuration set value to 0001 1000 for 16g
		MPU6050_writereg(accel, 0x23, 0xF8); // reg 35 FIFO enable set value to 1111 1000 for all sensors not slave
		MPU6050_writereg(accel, 0x37, 0x10); // reg 55 interrupt configuration set value to 0001 0000 for logic level high and read clear
		MPU6050_writereg(accel, 0x38, 0x01); // reg 56 interrupt enable set value to 0000 0001 data ready creates interrupt
		MPU6050_writereg(accel, 0x6A, 0x40); // reg 106 user control set value to 0100 0000 FIFO enable
		snprintf(outs,sizeof(outs),"done start \n\r");
		USART_tx_string(outs);
	}
	i2c_stop();
}



int main(){
	
	DDRD = 0xF0;
	DDRC = 0x00;
	double xa1, ya1, za1, xa2, ya2, za2;//declare averaged calibrated accelerometer values
	//initialize calibarition values
	int xi1 = 0;
	int xi2 = 0;
	int yi1 = 0;
	int yi2 = 0;
	int zi1 = 0;
	int zi2 = 0;
	//declare accelerometer value strings
	char x1s[10];
	char x2s[10];
	char y1s[10];
	char y2s[10];
	char z1s[10];
	char z2s[10];
	usart_init();//initialize usart
	i2c_init();     // init I2C interface
	_delay_ms(200);  // Wait for 200 ms.
	Init_MPU6050(MPU60501);    // front sensor init
	Init_MPU6050(MPU60502);    // rear sensor init
	_delay_ms(200);     // Wait for 200 ms.
	snprintf(outs,sizeof(outs),"6050 initialized \n\r");
	USART_tx_string(outs);
	for(int i = 0; i<10; i++)//get values for initial calibration
	{
			xi1 += MPU6050_signed_readreg(MPU60501,0x3B);   // read raw X acceleration from fifo
			xi2 += MPU6050_signed_readreg(MPU60502,0x3B);
			yi1 += MPU6050_signed_readreg(MPU60501,0x3D);   // read raw Y acceleration from fifo
			yi2 += MPU6050_signed_readreg(MPU60502,0x3D);
			zi1 += MPU6050_signed_readreg(MPU60501,0x3F);   // read raw Z acceleration from fifo
			zi2 += MPU6050_signed_readreg(MPU60502,0x3F);
			
	}
	//average values for calibration
	xi1 = xi1/10;
	xi2 = xi2/10;
	yi1 = yi1/10;
	yi2 = yi2/10;
	zi1 = zi1/10;
	zi2 = zi2/10;
	//Start infinite loop
	while(1){
		//grab 3 values, average, subtract calibration value, and divide by MSB

		xa1 = MPU6050_signed_readreg(MPU60501,0x3B)+MPU6050_signed_readreg(MPU60501,0x3B)+MPU6050_signed_readreg(MPU60501,0x3B);   // read raw X acceleration from fifo
		xa1 = ((xa1/3)-xi1)/2048.00;
		xa2 = MPU6050_signed_readreg(MPU60502,0x3B)+MPU6050_signed_readreg(MPU60502,0x3B)+MPU6050_signed_readreg(MPU60502,0x3B);
		xa2 = ((xa2/3)-xi2)/2048.00;
		ya1 = MPU6050_signed_readreg(MPU60501,0x3D)+MPU6050_signed_readreg(MPU60501,0x3D)+MPU6050_signed_readreg(MPU60501,0x3D);   // read raw Y acceleration from fifo
		ya1 = ((ya1/3)-yi1)/2048.00;
		ya2 = MPU6050_signed_readreg(MPU60502,0x3D)+MPU6050_signed_readreg(MPU60502,0x3D)+MPU6050_signed_readreg(MPU60502,0x3D); 
		ya2 = ((ya2/3)-yi2)/2048.00;
		za1 = MPU6050_signed_readreg(MPU60501,0x3F)+MPU6050_signed_readreg(MPU60501,0x3F)+MPU6050_signed_readreg(MPU60501,0x3F);   // read raw Z acceleration from fifo
		za1 = ((za1/3)-zi1)/2048.00;
		za2 = MPU6050_signed_readreg(MPU60502,0x3F)+MPU6050_signed_readreg(MPU60502,0x3F)+MPU6050_signed_readreg(MPU60502,0x3F);
		za2 = ((za2/3)-zi2)/2048.00;
		//convert doubles to printable strings
		dtostrf(xa1, 8, 6, x1s);
		dtostrf(xa2, 8, 6, x2s);
		dtostrf(ya1, 8, 6, y1s);
		dtostrf(ya2, 8, 6, y2s);
		dtostrf(za1, 8, 6, z1s);
		dtostrf(za2, 8, 6, z2s);
		//print out the values
		snprintf(outs,sizeof(outs),"{front accel x, T, ");
		USART_tx_string(outs);
		USART_tx_string(x1s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);
		snprintf(outs,sizeof(outs),"{front accel y, T, ");
		USART_tx_string(outs);
		USART_tx_string(y1s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);
		snprintf(outs,sizeof(outs),"{front accel z, T, ");
		USART_tx_string(outs);
		USART_tx_string(z1s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);
		snprintf(outs,sizeof(outs),"{rear accel x, T, ");
		USART_tx_string(outs);
		USART_tx_string(x2s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);
		snprintf(outs,sizeof(outs),"{rear accel y, T, ");
		USART_tx_string(outs);
		USART_tx_string(y2s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);
		snprintf(outs,sizeof(outs),"{rear accel z, T, ");
		USART_tx_string(outs);
		USART_tx_string(z2s);
		snprintf(outs,sizeof(outs),"} \n\r");
		USART_tx_string(outs);		
	} 
	
	return 0;
} 